#ifndef URCONTROLSTAMP_HPP
#define URCONTROLSTAMP_HPP

#include "urcontrola.hpp"
#include <opencv2/opencv.hpp>
#include "deskA.hpp"
#include "imagetoplane.hpp"

#include <thread>
#include <chrono>

class URControlStamp : public URControlA
{
protected:

    static bool Locate(cv::Vec2d paperCorner[4], const cv::Mat &frame, const cv::Mat &mask, double thres)
    {
        cv::Mat paper;
        cv::threshold(frame,paper,thres,255,cv::THRESH_BINARY);
        cv::bitwise_and(paper, mask, paper);
        std::vector<cv::Point> bgCandidate(1,cv::Point(0,0));
        cv::Mat bw;
        ObjectMask::FloodFromPoints(bw, paper, bgCandidate);
        //    cv::bitwise_not(bw, bw);
        cv::imshow("bw",bw);
        cv::waitKey(1);
        cv::Point2f corner[4];
        if(!DeskA::FindDesk(bw, corner))
        {
            std::cout<<"fail to locate rect\n"<<std::flush;
            return false;
        }
        for(size_t i=0;i<4;i++)
        {
            paperCorner[i][0]=corner[i].x;
            paperCorner[i][1]=corner[i].y;
        }

        std::cout<<paperCorner[0]<<"\n"
                                <<paperCorner[1]<<"\n"
                               <<paperCorner[2]<<"\n"
                              <<paperCorner[3]<<"\n"
                             <<std::flush;

        return true;
    }

    static cv::Mat GetFrame(int cameraIndex=0, int n=10)
    {
        cv::VideoCapture c(cameraIndex);

        cv::Mat frame;
        for(int i=0;i<n;i++)
            c.read(frame);

        std::cout<<frame.size()<<"\n";

        c.release();

        if(frame.empty())
        {
            std::cout<<"fail to get frame\n"<<std::flush;
            return cv::Mat();
        }

        cv::Mat gray;
        cv::cvtColor(frame,gray,CV_BGR2GRAY);
        return gray;
    }

    static bool Locate(cv::Vec2d paperCorner[4], const cv::Mat &mask, int cameraIndex=0, double thres=128)
    {
        cv::VideoCapture c(cameraIndex);

        cv::Mat frame;
        for(int i=0;i<10;i++)
            c.read(frame);

        std::cout<<frame.size()<<"\n";

        c.release();

        if(frame.empty())
        {
            std::cout<<"fail to get frame\n"<<std::flush;
            return false;
        }

        cv::Mat gray;
        cv::cvtColor(frame,gray,CV_BGR2GRAY);
        cv::imshow("gray", gray);
        cv::waitKey(1);

        return Locate(paperCorner, gray, mask, thres);
    }


    static void OnMouse(int event, int x, int y, int flag, void* pdata)
    {


        if( event != cv::EVENT_LBUTTONDBLCLK )
            return;

        std::cout<<"callback\n"<<std::flush;
        URControlStamp *pur=(URControlStamp*)pdata;

        pur->ForceTouchPoint(cv::Vec2d(x,y), cv::Vec3d(0,0,0.21), 0.1);

        pur->SetPose(-0.0425296,-0.277517,0.388848,-1.27595,2.85684,-0.0391458, .2);

        pur->LoadAndPlay("RG2_Testing/open_30");

        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

public:
    ImageToPlane i2p;

    URControlStamp(std::string ipPC="192.168.1.14", std::string ipUR="192.168.1.13", unsigned short portPC=1235, unsigned short portUR=30003, unsigned short modbusPort=502)
        : URControlA(ipPC, ipUR, portPC, portUR, modbusPort)
    {
#if 0
        cv::Vec6d headPosBase[4]={
            cv::Vec6d(0.512474,-0.187912,0.226423,-2.98454,0.697973,-0.399517),
            cv::Vec6d(0.214651,-0.197416,0.225197,3.13498,0.0731075,0.0959954),
            cv::Vec6d(0.228239,0.296933,0.225976,2.36381,-2.06457,0.0251457),
            cv::Vec6d(0.496013,0.254633,0.21989,-2.88894,-0.0586657,-0.468388)
        };

        cv::Vec2d referencePixel[4]={
            cv::Vec2d(807,148)/1.5,
            cv::Vec2d(744,492)/1.5,
            cv::Vec2d(234,485)/1.5,
            cv::Vec2d(170,148)/1.5
        };
#endif

        cv::Vec6d headPosBase[4]={
            cv::Vec6d(0.244364,-0.179697,0.221591,-1.09127,2.90713,0.183697),
            cv::Vec6d(0.443805,-0.144863,0.225187,-1.49533,2.59919,-0.258374),
            cv::Vec6d(0.415067,0.212753,0.219907,-1.23241,2.70302,-0.296297),
            cv::Vec6d(0.222613,0.175098,0.220047,-1.10212,2.87574,0.0419509)
        };

        cv::Vec2d referencePixel[4]={
            //cv::Vec2d(429,9),
            //cv::Vec2d(123,4),
            //cv::Vec2d(152,455),
            //cv::Vec2d(450,445)
            cv::Vec2d(430,36),
            cv::Vec2d(121,24),
            cv::Vec2d(142,470),
            cv::Vec2d(436,471)
        };




        i2p.Calibrate(cv::Vec3d(0,0,0.21), headPosBase, referencePixel, cv::Size(640,480), false);

    }


    void TouchRectangleCenter(cv::Vec2d rectangleCorner[4], cv::Vec3d fingerXYZ, double v=0.2)
    {
        cv::Vec6d pos=i2p.RectangleCornerPixelCoordinateToHeadPos(fingerXYZ, rectangleCorner);

        SetPose(pos[0],pos[1],pos[2]+0.06,pos[3],pos[4],pos[5], v);

        SetPose(pos[0],pos[1],pos[2]+0.02,pos[3],pos[4],pos[5], v);

        SetPose(pos[0],pos[1],pos[2]+0.06,pos[3],pos[4],pos[5], v);
    }

    void ForceTouchRectangleCenter(cv::Vec2d rectangleCorner[4], cv::Vec3d fingerXYZ, double v=0.2)
    {
        cv::Vec6d pos=i2p.RectangleCornerPixelCoordinateToHeadPos(fingerXYZ, rectangleCorner);

        SetPose(pos[0],pos[1],pos[2]+0.08,pos[3],pos[4],pos[5], v);

        //LoadAndPlay("RG2_Testing/open_30");
        //std::this_thread::sleep_for(std::chrono::seconds(1));

//        LoadAndPlay2("RG2_Testing/grab_capa");
//        LoadAndPlay("fff");
        //std::this_thread::sleep_for(std::chrono::seconds(1));
        //LoadAndPlay("RG2_Testing/close");


        //std::this_thread::sleep_for(std::chrono::seconds(1));

//        SetPose(pos[0],pos[1],pos[2]+0.08,pos[3],pos[4],pos[5], v); //Return to base-station
    }

    void TouchPoint(cv::Vec2d point, cv::Vec3d fingerXYZ, double v=0.2)
    {
        cv::Vec6d pos=i2p.PixelCoordinateToHeadPos(fingerXYZ, point);

        SetPose(pos[0],pos[1],pos[2]+0.06,pos[3],pos[4],pos[5], v);

        SetPose(pos[0],pos[1],pos[2]+0.04,pos[3],pos[4],pos[5], v);

        SetPose(pos[0],pos[1],pos[2]+0.06,pos[3],pos[4],pos[5], v);
    }



    void ForceTouchPoint(cv::Vec2d point, cv::Vec3d fingerXYZ, double v=0.2)
    {
        cv::Vec6d pos=i2p.PixelCoordinateToHeadPos(fingerXYZ, point);

        SetPose(pos[0],pos[1],pos[2]+0.07,pos[3],pos[4],pos[5], v);

        LoadAndPlay("RG2_Testing/open_30");

        //set_gripper(30);
//        std::this_thread::sleep_for(std::chrono::seconds(1));

        //LoadAndPlay("fff");

//         std::this_thread::sleep_for(std::chrono::seconds(1));

         //LoadAndPlay("RG2_Testing/close");


//         std::this_thread::sleep_for(std::chrono::seconds(1));

        //SetPose(pos[0],pos[1],pos[2]+0.07,pos[3],pos[4],pos[5], v);
    }


    void Touch()
    {
        double printLength=0.235;

        cv::Vec2d paperPixel[4];
        if(Locate(paperPixel, i2p.mask))
        {
            TouchRectangleCenter(paperPixel, cv::Vec3d(0,0,printLength), 0.4);
        }
        SetPose(-0.0489175,0.367352,0.340857,1.05044,-2.94398,-0.0184143, .8);

    }

    void ForceTouch()
    {
        double printLength=0.235;

        cv::Vec2d paperPixel[4];
        if(Locate(paperPixel, i2p.mask))
        {
            ForceTouchRectangleCenter(paperPixel, cv::Vec3d(0,0,printLength), 0.4);
        }
        SetPose(-0.0489175,0.367352,0.340857,1.05044,-2.94398,-0.0184143, .2);

    }

    void Test(std::string winname="m")
    {
        cv::Mat m=GetFrame();
        cv::namedWindow(winname);
        cv::setMouseCallback(winname, OnMouse, this);
        cv::imshow(winname,m);
        cv::waitKey();
    }

};

#endif // URCONTROLSTAMP_HPP
